package mskruch.gps.util;

import gpstools.Location;
import mskruch.gps.PointFactory;
import mskruch.gps.Position;
import mskruch.gps.calculation.Calculations;
import mskruch.gps.calculation.PositionsCalculator;

public class TrackUtil {
    /**
     * Find loop in points, given start index
     * 
     * @param points
     *            track points
     * @param startIndex
     *            starting point index
     * @param minimumDistance
     *            minimum distance for a loop
     * 
     * @return ending point index or <code>-1</code> if not found
     */
    public static int findLoopEnd(Position[] points, int startIndex, double minimumDistance) {
        return _findLoopEnd(points, startIndex, minimumDistance);
    }

    public static int findLoopEnd(Location[] points, int startIndex, double minimumDistance) {
        Position[] positions = new Position[points.length];
        int i = 0;
        for (Location location : points) {
            positions[i++] = PointFactory.newPosition(location.getLongitude(), location.getLatitude());
        }
        return _findLoopEnd(positions, startIndex, minimumDistance);
    }

    private static int _findLoopEnd(Position[] points, int startIndex, double minimumDistance) {
        Position finish = points[startIndex];

        int skip = 0;
        final PositionsCalculator pc = Calculations.newPositionsCalculator();
        while (pc.getDistance() < minimumDistance) {
            pc.add(points[skip++]);
        }

        for (int i = skip; i < points.length; i++) {
            Position prev = points[i - 1]; /* previous point */
            Position curr = points[i]; /* current point */

            /* http://www.math.edu.pl/kat-miedzy-wektorami */
            double px = prev.getLongitude();
            double sx = finish.getLongitude();
            double cx = curr.getLongitude();
            double py = prev.getLatitude();
            double sy = finish.getLatitude();
            double cy = curr.getLatitude();
            double dprev = Math.sqrt(Math.pow(px - sx, 2) + Math.pow(py - sy, 2));
            double dcurr = Math.sqrt(Math.pow(cx - sx, 2) + Math.pow(cy - sy, 2));
            double cos = ((px - sx) * (cx - sx) + (py - sy) * (cy - sy)) / (dprev * dcurr);

            /* angle between two vectors */
            double angle = Math.acos(cos);

            if (angle > Math.PI / 4)
                return i;
        }

        if (points.length < 3) {
            return -1;/* loop not found */
        }

        Position last = points[points.length - 1];
//        Position beforeLast = points[points.length - 2];

        if (last.distanceTo(finish) < 5) {
            return points.length - 1;
        }
//        double a2 = Math.pow(last.distanceTo(beforeLast), 2);
//        double b2 = Math.pow(last.distanceTo(finish), 2);
//        double c2 = Math.pow(beforeLast.distanceTo(finish), 2);
//
//        double x = (a2 + b2) / c2;
//
//        if (Math.abs(1 - x) < 0.05) {
//            return points.length - 1;
//        }

        return -1; /* loop not found */
    }
    
    @Deprecated
    public static int findTrackEnd(Position[] points, int startFrom, Position finish, double tolerance) {
        double distanceToFinish = points[startFrom].distanceTo(finish);
        boolean found = false;
        int nearest = startFrom;
        for (int i = startFrom + 1; i < points.length; i++) {
            Position curr = points[i];
            double distance = curr.distanceTo(finish);
            if (distance < tolerance) {
                found = true;
                if (distance < distanceToFinish) {
                    nearest = i;
                }
                System.out.println(distance);
                if (distance > distanceToFinish)
                    /* oddalanie */
                    return i;
            }
            distanceToFinish = distance;
        }
        if (found) {
            return nearest + 1;
        }
        return -1;
    }

}
